/*
bea_u920.c
offer bea u920 lidar config and get data function 
designed by chenwenbo

----------------------------
2020-11-18 10:00:00
create file
----------------------------
*/

#include "bea_u920.h"
#include "../../bsp/cli_cmd.h"
#include "../../../Include/aos/kernel.h"
#include "../../bsp/xp_uart.h"
#include "stdio.h"
#include "string.h"
#include "math.h"


int xp_bea_store_config(Type_Bea_U920_Def *const pBea_U920);
u16 xp_bea_get_data_len(Type_Bea_U920_Def const *pBea_U920, u16 cmd);
int xp_bea_measure_data_decode(Type_Bea_U920_Def *const pBea_U920);
int xp_bea_identity_data_decode(Type_Bea_U920_Def *const pBea_U920);
int xp_bea_data_decode(Type_Bea_U920_Def *const pBea_U920);
int xp_bea_sampling_state_machine(Type_Bea_U920_Def *const pBea_U920);
int xp_bea_update_params_to_lidar(Type_Bea_U920_Def *const pBea_U920);
int xp_bea_set_plane_params_to_mem(Type_Bea_U920_Def *const pBea_U920, u8 plane_id, u8 angleStart, u8 angleEnd, u8 spots_num);
int xp_bea_get_measurements(Type_Bea_U920_Def *const pBea_U920, u8 type);
int xp_bea_fram_encode_send(Type_Bea_U920_Def *const pBea_U920, u16 cmd, u8 *pData, u16 dataSize);
int xp_uart_send(Type_Bea_U920_Def *const pBea_U920, u8 *pData, u16 size, u16 timeout);
int xp_uart_recv(Type_Bea_U920_Def *const pBea_U920, u8 *pData, u16 size, u16 timeout);
u16 xp_bea_crc(u8 *data, int len);
void xp_bea_plane_analysis(Type_Bea_U920_Def *const pBea_U920);
void xp_bea_get_u920_data_thread(void *arg);
void xp_bea_process_u920_data_thread(void *arg);

static aos_sem_t process_sem;
const u8 sync_filed[] = {0xBE, 0xA0, 0x12, 0x34, 0x02, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00};
Type_Bea_U920_Def Bea_U920;


/*function: xp_bea_default_params_init_to_mem()
 *Used to initialize the radar default parameters
 *note that this will not be sent to the device
 *param: 
 *	pBea_U920: lidar handle
 *return: void
*/
void xp_bea_default_params_init_to_mem(Type_Bea_U920_Def *pBea_U920)
{
	//memset( pBea_U920, 0, sizeof(Type_Bea_U920_Def) );

	//config mode: send MDI only (each spots distance value occupy 2bytes)
	pBea_U920->paramSet.meas_info = 0,

	//config angle rang: (unit: 0.01deg)
	pBea_U920->paramSet.plane[0].angle_first_facet = 0;
	pBea_U920->paramSet.plane[0].angle_last_facet = 10000;
	pBea_U920->paramSet.plane[0].spots_number_facet = 100;

	pBea_U920->paramSet.plane[1].angle_first_facet = 0;
	pBea_U920->paramSet.plane[1].angle_last_facet = 10000;
	pBea_U920->paramSet.plane[1].spots_number_facet = 100;

	pBea_U920->paramSet.plane[2].angle_first_facet = 0;
	pBea_U920->paramSet.plane[2].angle_last_facet = 10000;
	pBea_U920->paramSet.plane[2].spots_number_facet = 100;

	pBea_U920->paramSet.plane[3].angle_first_facet = 0;
	pBea_U920->paramSet.plane[3].angle_last_facet = 10000;
	pBea_U920->paramSet.plane[3].spots_number_facet = 100;

	//config frame format: 
	pBea_U920->paramSet.ntc_inFrame = 1;
	pBea_U920->paramSet.count_inFrame = 1;
	pBea_U920->paramSet.can_inFrame = 0;
	pBea_U920->paramSet.facet_id_inFrame = 1;
	
	//config heartbeat period (s)
	pBea_U920->paramSet.heartbeat_period = 0;
	//config baudrate: (4: 921600)
	pBea_U920->paramSet.baudrate_sel = 4;
}

/*function: xp_bea_u920_init()
 *Used to initialize the lidar communication interface 
 *set the default parameters, and start the two tasks of 
 *data reception and processing.
 *param: 
 *	arg: In the future, if multiple lidars are used, 
		the lidars handle will be passed in.
 *return: void
*/
int xp_bea_u920_init(void *arg, u8 port, u32 speed){

    char buf[30];
	
    Bea_U920.uart.port = port;
    Bea_U920.uart.config.baud_rate = speed;
    Bea_U920.uart.config.data_width = DATA_WIDTH_8BIT;
    Bea_U920.uart.config.parity = NO_PARITY;
    Bea_U920.uart.config.stop_bits = STOP_BITS_1;
    Bea_U920.uart.config.flow_control = FLOW_CONTROL_DISABLED;
    Bea_U920.uart.config.mode = MODE_TX_RX;

    int sta = hal_uart_init(&Bea_U920.uart);
    if(sta != 0){
        perr("bea init fail~ uart init fail");
        return -1;
    }

	//Used to receive task notification processing tasks
	aos_sem_new(&process_sem, 0);
	
    sprintf(buf, "bea data,port=%d", port);
	
    
    if(aos_task_new(buf, xp_bea_get_u920_data_thread, &Bea_U920, 2000) != 0){
        perr("bea init fail~ create get data thread fail~");
        return -2;
    }
	
	
    if(aos_task_new(buf, xp_bea_process_u920_data_thread, &Bea_U920, 4000) != 0){
        perr("bea init fail~ create process data thread fail~");
        return -2;
    }
	
    println("bea lidar init success~");
    return 0;
}

/***********************************************************************/
void xp_bea_get_u920_data_thread(void *arg){
	
	Type_Bea_U920_Def *pBea_U920 = arg;
    int sta = 0;
	
	xp_bea_default_params_init_to_mem( pBea_U920 );
	print("xp_bea_set_params...\r\n");
    xp_bea_update_params_to_lidar( pBea_U920 );
	
	print("xp_bea_store_config...\r\n");
    xp_bea_store_config( pBea_U920  );
	aos_msleep(140);	//wait param store

    while(1){
        
		sta = xp_bea_sampling_state_machine( pBea_U920 );		//Send collection commond regularly
		if (0 == sta){
			/*Give the received data to the processing task, 
			  the processing task must release this memory*/
			pBea_U920->frameProcess.pData = pBea_U920->frameRecv.pData;
			pBea_U920->frameProcess.size = pBea_U920->frameRecv.size;
			//aos_sem_signal(&process_sem);	//notify process task, have new data
			pBea_U920->isProcess = true;
		}
        aos_msleep(10);
    }
}

void xp_bea_process_u920_data_thread(void *arg){
	
	Type_Bea_U920_Def *pBea_U920 = arg;
    while(1){
		
		//aos_sem_wait(&process_sem, AOS_WAIT_FOREVER);
		
		if (pBea_U920->isProcess){
			if (0 == xp_bea_data_decode( pBea_U920 )){
				xp_bea_plane_analysis( pBea_U920 );
			}
		
			/*Whether the analysis is successful, must release this memory*/
			aos_free(pBea_U920->frameProcess.pData);
			pBea_U920->isProcess = false;
		}
        aos_msleep(1);
    }
}
/***********************************************************************/

int xp_bea_sampling_state_machine(Type_Bea_U920_Def *const pBea_U920)
{
	if(aos_now_ms() - pBea_U920->timeStamp > SAMPLING_PERIOD_MS){
		
		pBea_U920->timeStamp = aos_now_ms();

		switch(pBea_U920->state)
		{
			case 0:
				xp_bea_set_plane_params_to_mem(pBea_U920, 0, 0, 100, 250);
				xp_bea_set_plane_params_to_mem(pBea_U920, 1, 0, 100, 100);
				xp_bea_set_plane_params_to_mem(pBea_U920, 2, 0, 100, 75);
				xp_bea_set_plane_params_to_mem(pBea_U920, 3, 0, 100, 75);
				xp_bea_update_params_to_lidar(pBea_U920);
				pBea_U920->state = 0;		//...?
			break;
			
			case 1:
				xp_bea_set_plane_params_to_mem(pBea_U920, 0, 0, 100, 100);
				xp_bea_set_plane_params_to_mem(pBea_U920, 1, 0, 100, 250);
				xp_bea_set_plane_params_to_mem(pBea_U920, 2, 0, 100, 75);
				xp_bea_set_plane_params_to_mem(pBea_U920, 3, 0, 100, 75);
				xp_bea_update_params_to_lidar(pBea_U920);
				pBea_U920->state = 0;
			break;
			
			
			default:
				pBea_U920->state = 0;
			break;
		}
		
		u8 Buf[58];
		xp_uart_recv(pBea_U920, Buf, 58, 100);
		
		xp_bea_get_measurements(pBea_U920, 0);
	
		u16 len = xp_bea_get_data_len(pBea_U920, CMD_GET_MEASUREMENTS);
		pBea_U920->frameRecv.pData = aos_malloc( len );
		if (NULL != pBea_U920->frameRecv.pData){
			
			int sta = xp_uart_recv(pBea_U920, pBea_U920->frameRecv.pData, len, 100);
			if (sta > 0){
				if( sta < len ){
					//print("maybe the recv waiting time is too short\r\n");
				}
				pBea_U920->frameRecv.size = sta;
				//print("Recv commplite %d %d\r\n", sta, len);
				return 0;
			}
		}
		return -2;
	}
	return -1;
}

int xp_bea_data_decode(Type_Bea_U920_Def *const pBea_U920)
{
	if (NULL != pBea_U920->frameProcess.pData \
		&& pBea_U920->frameProcess.size >= ( sizeof(sync_filed) + 2 + 2) ){
		
		u16 crc16;

		memcpy(&crc16, pBea_U920->frameProcess.pData + pBea_U920->frameProcess.size - 2, sizeof(u16));
		
		if ( xp_bea_crc(pBea_U920->frameProcess.pData, pBea_U920->frameProcess.size - 2) == crc16 ){		//check the crc
			
			switch ( GET_CMD(pBea_U920->frameProcess.pData) )
			{
				case CMD_GET_MEASUREMENTS:
					return xp_bea_measure_data_decode( pBea_U920 );
				break;
				
				case CMD_GET_IDENTITY:
					return xp_bea_identity_data_decode( pBea_U920 );
				break;
				
				default:
					print("This CMD(0x%04X) is not decode function\r\n", GET_CMD(pBea_U920->frameProcess.pData));
					return -3;
				break;
			}
		}else{
			print("crc not match\r\n");
			return -2;
		}
	}
	print("maybe is loss the recv data\r\n");
	return -1;
}


int xp_bea_measure_data_decode(Type_Bea_U920_Def *const pBea_U920)
{
	u16 index = sizeof(sync_filed) + 2;	//Size(SYNC + CMD)
	
	if (pBea_U920->paramSet.can_inFrame){
		memcpy(pBea_U920->canBuf, pBea_U920->frameProcess.pData + index, 8);
		index += 8;
	}
	
	if (pBea_U920->paramSet.count_inFrame){
		memcpy(&pBea_U920->count, pBea_U920->frameProcess.pData + index, sizeof(u16));
		index += sizeof(u16);
	}
	
	if (pBea_U920->paramSet.ntc_inFrame){
		memcpy(&pBea_U920->temperture, pBea_U920->frameProcess.pData + index, sizeof(u16));
		index += sizeof(u16);
	}
	
	if (pBea_U920->paramSet.facet_id_inFrame){
		memcpy(&pBea_U920->facet, pBea_U920->frameProcess.pData + index, sizeof(u8));
		index += sizeof(u8);
	}

	for (u8 i = 0; i < 4; i++){
		if (0 != pBea_U920->paramSet.plane[i].spots_number_facet){
			print("copy faceId: %d to plane: %d spot size: %d\r\n", pBea_U920->facet, i, pBea_U920->plane[i].spotSize);
			
			pBea_U920->plane[i].spotSize = pBea_U920->paramSet.plane[i].spots_number_facet;
			memcpy(pBea_U920->plane[i].Data, pBea_U920->frameProcess.pData + index, pBea_U920->plane[i].spotSize * sizeof(u16));
			pBea_U920->plane[i].isUpdate = true;
			

			
			//print("Plane_%d (%d) first_byte: %04X %d(index: %d)\r\n", \
						  i, pBea_U920->plane[i].spotSize, pBea_U920->plane[i].Data[0], pBea_U920->plane[i].Data[0], index);
			index += pBea_U920->paramSet.plane[i].spots_number_facet * sizeof(u16);
			if (pBea_U920->paramSet.facet_id_inFrame){
				pBea_U920->facet = *(pBea_U920->frameProcess.pData + index);
				index++;
			}
		}
	}
	
	return 0;
}

int xp_bea_identity_data_decode(Type_Bea_U920_Def *const pBea_U920)
{
	u8 index = sizeof(sync_filed) + 2;	//Size(SYNC + CMD)
	memcpy(&pBea_U920->identity, pBea_U920->frameProcess.pData + index, xp_bea_get_data_len(pBea_U920, CMD_GET_IDENTITY) );
	return 0;
}

int xp_bea_fram_encode_send(Type_Bea_U920_Def *const pBea_U920, u16 cmd, u8 *pData, u16 dataSize)
{
	int ret = -1;
	u16 fram_len = sizeof(sync_filed) + 2 + dataSize + 2;	// (SYNC + CMD + DATA + CHK) 
	
	u8 *pBuf = (u8*)aos_malloc(fram_len);
	if (NULL != pBuf){
		memcpy(pBuf, sync_filed, sizeof(sync_filed));
		memcpy(pBuf + sizeof(sync_filed), &cmd, sizeof(cmd));
		
		if (NULL != pData && (0 != dataSize)){
			memcpy(pBuf + sizeof(sync_filed) + sizeof(cmd), pData, dataSize);
		}
		memcpy(&pBuf[5], &fram_len, sizeof(fram_len));
			
		u16 crc16 = xp_bea_crc(pBuf, fram_len - 2);
		pBuf[fram_len - 2] = crc16;
		pBuf[fram_len - 1] = crc16 >> 8;
		
		xp_uart_send(pBea_U920, pBuf, fram_len, 100);
		ret = 0;
	}
	aos_free(pBuf);
	return ret;
}

void xp_bea_plane_analysis(Type_Bea_U920_Def *const pBea_U920)
{
	
	for (u8 n = 0; n < 4; n++){
		if (pBea_U920->plane[n].isUpdate){
			pBea_U920->plane[n].isUpdate = false;
		}else{
			continue;
		}
		
		float K = (pBea_U920->paramSet.plane[n].angle_last_facet - pBea_U920->paramSet.plane[n].angle_first_facet) / \
															   (float)pBea_U920->plane[n].spotSize / 100.0f;
		//have some function not achieve.....
		for (u16 i = 0; i < pBea_U920->plane[n].spotSize / 2; i++){
			float theta = 50 - i * K;
			pBea_U920->plane[n].Data[i] = (u16)(cos(theta * PI / 180.0f) * pBea_U920->plane[n].Data[i]);
			pBea_U920->plane[n].Data[pBea_U920->plane[n].spotSize - i] = (u16)(cos(theta * PI / 180.0f) * pBea_U920->plane[n].Data[pBea_U920->plane[n].spotSize - i]);
			if (n == 0 || n == 3){
				pBea_U920->plane[n].Data[i] = (u16)(cos(3 * PI / 180.0f) * pBea_U920->plane[n].Data[i]);
			}else if(n == 1 || n == 2){
				pBea_U920->plane[n].Data[i] = (u16)(cos(1 * PI / 180.0f) * pBea_U920->plane[n].Data[i]);
			}
		}
		
		for (u16 i = pBea_U920->plane[n].spotSize / 2; i < pBea_U920->plane[n].spotSize; i++){
			float theta = (i - pBea_U920->plane[n].spotSize / 2) * K;
			pBea_U920->plane[n].Data[i] = (u16)(cos(theta * PI / 180.0f) * pBea_U920->plane[n].Data[i]);
			if (n == 0 || n == 3){
				pBea_U920->plane[n].Data[i] = (u16)(cos(3 * PI / 180.0f) * pBea_U920->plane[n].Data[i]);
			}else if(n == 1 || n == 2){
				pBea_U920->plane[n].Data[i] = (u16)(cos(1 * PI / 180.0f) * pBea_U920->plane[n].Data[i]);
			}
		}
		
		#if 0		//Log ware
		print("Plane:%d\r\n", n);
		for (u16 i = 0; i < pBea_U920->plane[n].spotSize; i +=10){
			for (u16 j = 0; j < (pBea_U920->plane[n].Data[i]) / 10; j++){
				print(" ");
			}
			print("*\r\n");
		}
		#endif
		
		#if 1		//Log data
		print("Plane:%d\r\n", n);
		for (u16 i = 0; i < pBea_U920->plane[n].spotSize; i ++){
			print("%4d ", pBea_U920->plane[n].Data[i]);
		}
		print("\r\n");
		#endif
	}
}

u16 xp_bea_get_data_len(Type_Bea_U920_Def const *pBea_U920, u16 cmd)
{
	switch (cmd)
	{
		case CMD_GET_MEASUREMENTS:
			return ( sizeof(sync_filed) + sizeof(u16) + \
					(pBea_U920->paramSet.can_inFrame 		? (8) : (0)) + \
					(pBea_U920->paramSet.ntc_inFrame 		? (2) : (0)) + \
					(pBea_U920->paramSet.count_inFrame 		? (2) : (0)) + \
					(pBea_U920->paramSet.facet_id_inFrame 	? (
						( pBea_U920->paramSet.plane[0].spots_number_facet ? (1) : (0 ) ) + \
						( pBea_U920->paramSet.plane[1].spots_number_facet ? (1) : (0 ) ) + \
						( pBea_U920->paramSet.plane[2].spots_number_facet ? (1) : (0 ) ) + \
						( pBea_U920->paramSet.plane[3].spots_number_facet ? (1) : (0 ) ) \
																) : (0)) + \
					( pBea_U920->paramSet.plane[0].spots_number_facet * 2 ) + \
					( pBea_U920->paramSet.plane[1].spots_number_facet * 2 ) + \
					( pBea_U920->paramSet.plane[2].spots_number_facet * 2 ) + \
					( pBea_U920->paramSet.plane[3].spots_number_facet * 2 ) + \
					 sizeof(u16) );
		break;
		
		case CMD_GET_IDENTITY:
			//Due to the byte alignment of the structure, "Type_Recv_Identity_Def" size needs to be reduced by two
			return ( sizeof(Type_Recv_Identity_Def) - 2 );
		break;
	}
	return 1000;
}

int xp_bea_set_plane_params_to_mem(Type_Bea_U920_Def *const pBea_U920, u8 plane_id, u8 angleStart, u8 angleEnd, u8 spots_num)
{
	if (plane_id >= 0 && plane_id < 4){
		pBea_U920->paramSet.plane[plane_id].angle_first_facet = angleStart * 100;	//unit: 0.01deg
		pBea_U920->paramSet.plane[plane_id].angle_last_facet = angleEnd * 100;		//unit: 0.01deg
		pBea_U920->paramSet.plane[plane_id].spots_number_facet = spots_num;
	}
	return 0;
}

int xp_bea_update_params_to_lidar(Type_Bea_U920_Def *const pBea_U920)
{
	//Due to the byte alignment of the structure, param "size" needs to be reduced by one
	return xp_bea_fram_encode_send(pBea_U920, CMD_STE_PARAMETERS, (u8 *)&pBea_U920->paramSet, sizeof(Type_Set_Parameters_Def) - 1);
}

int xp_bea_get_measurements(Type_Bea_U920_Def *const pBea_U920, u8 type)
{
	Type_Get_Measurements_Def params;
	params.meas_mode = type;
	return xp_bea_fram_encode_send(pBea_U920, CMD_GET_MEASUREMENTS, (u8 *)&params, sizeof(params));
}

int xp_bea_store_config(Type_Bea_U920_Def *const pBea_U920)
{
	return xp_bea_fram_encode_send(pBea_U920, CMD_STORE_PARAMETERS, NULL, 0);
}

int xp_uart_send(Type_Bea_U920_Def *const pBea_U920, u8 *pData, u16 size, u16 timeout)
{
	int sta;
	
	sta = hal_uart_send(&pBea_U920->uart, pData, size, 40);
    if(sta != 0){
        //print("send err\r\n");
        return -1;
    }
	return 0;
}

int xp_uart_recv(Type_Bea_U920_Def *const pBea_U920, u8 *pData, u16 size, u16 timeout)
{
	int sta;
	u32 recvLen = 0;
	sta = hal_uart_recv_II(&pBea_U920->uart, pData, size, &recvLen, timeout);
    if(sta != 0){
        //print("recv err %d\r\n", sta);
        return -2;
    }
    return recvLen;
}

u16 xp_bea_crc(u8 *data,int len)
{
	uint16_t crc = 0;
	uint16_t i, j;
	for(i = 0; i < len; i++)
	{
		crc ^= (uint16_t)(data[i] << 8);
		for(j = 0; j < 8; j++)
		{
			if((crc & 0x8000) != 0){
				crc = (uint16_t)((crc << 1) ^ BEA_key);
			}else{
				crc <<= 1;
			}
		}
	}
	return crc;
}
